﻿/// <summary>
/// Developer: CHOOI KAH WAI
/// Last Modified: 23/10/2009
/// </summary>

using System;
using System.Collections.Generic;
using System.Collections;
using ARTKPManagedWrapper;
using ARSS.Util;
using System.Runtime.InteropServices;

namespace ARSS.Components
{
    /// <summary>
    /// ARToolkitPlus Component
    /// Implementation of ARToolkitPlus was refered to /wpfAugReal
    /// /wpfAugReal (c) 2007 by casey chesnut, brains-N-brawn LLC
    /// /wpfAugReal can be obtained from http://www.mperfect.net/wpfAugReal/
    /// </summary>
    public class ARCom_ARTKP : ARCom
    {
        #region Variables

        /// <summary>
        /// Board Tracker
        /// </summary>
        private IntPtr boardTracker;

        /// <summary>
        /// Actions Tracker
        /// </summary>
        private IntPtr actionTracker;

        /// <summary>
        /// List of action markers' ID
        /// </summary>
        private ArrayList actionMarkerIDs;

        #region Constants

        private const string CAMERA_CALIBRATION_PATH = "data/no_distortion.cal";

        #endregion

        #endregion

        #region Functions

        /// <summary>
        /// Initialize AR trackers
        /// </summary>
        /// <param name="ptr">Tracker object</param>
        /// <param name="configPath">AR configuration file path</param>
        /// <param name="vidWidth">Video Width</param>
        /// <param name="vidHeight">Video Height</param>
        /// <param name="pixelFmt">Pixel Format</param>
        private void InitTracker(ref IntPtr ptr, string configPath, int vidWidth, int vidHeight, int pixelFmt)
        {
            // Construct the tracker
            ptr = ArManWrap.ARTKPConstructTrackerMulti(-1, vidWidth, vidHeight);

            if (ptr == IntPtr.Zero)
                throw new Exception("ARTKPConstructTrackerMulti failed.");

            ArManWrap.ARTKPSetPixelFormat(ptr, pixelFmt);

            if (ArManWrap.ARTKPInitMulti(ptr, CAMERA_CALIBRATION_PATH, configPath, 1.0f, 2000.0f) != 0)
                throw new Exception("Tracker initialization failed.");

            // Using simple marker set
            ArManWrap.ARTKPSetMarkerMode(ptr, (int)ArManWrap.MARKER_MODE.MARKER_ID_SIMPLE);
            ArManWrap.ARTKPSetBorderWidth(ptr, 0.125f);

            // Auto threshold
            ArManWrap.ARTKPActivateAutoThreshold(ptr, true);

            // Use LUT Undisortion mode
            ArManWrap.ARTKPSetUndistortionMode(ptr, (int)ArManWrap.UNDIST_MODE.UNDIST_LUT);

            // Use advance detection method
            ArManWrap.ARTKPSetUseDetectLite(ptr, false);

        }

        

        #endregion

        #region ARCom Implementations

        #region Delegates / Events

        public override event ARSS.Components.Coordinator.ARUpdateHandler ARUpdateEvent;

        #endregion

        #region Functions 

        public override void Init(int vidWidth, int vidHeight, string boardCfg, string actionCfg, Coordinator co, CamVideoInfo cam)
        {
            CalcResults = new ArrayList();
            this.actionMarkerIDs = new ArrayList();


            int pixelFmt;
            if (cam.VideoPixelFormat == System.Drawing.Imaging.PixelFormat.Format24bppRgb)
                pixelFmt = (int)ArManWrap.PIXEL_FORMAT.PIXEL_FORMAT_RGB;
            else
                pixelFmt = (int)ArManWrap.PIXEL_FORMAT.PIXEL_FORMAT_ABGR;

            // Init board tracker
            InitTracker(ref this.boardTracker, boardCfg, vidWidth, vidHeight, pixelFmt);

            // Init action tracker
            InitTracker(ref this.actionTracker, actionCfg, vidWidth, vidHeight, pixelFmt);
            // Get list of action markers
            IntPtr config = ArManWrap.ARTKPGetMultiMarkerConfig(this.actionTracker);
            ArManWrap.ARMultiMarkerInfoT configStruct = (ArManWrap.ARMultiMarkerInfoT)Marshal.PtrToStructure(config, typeof(ArManWrap.ARMultiMarkerInfoT));
            IntPtr currentMarker = configStruct.marker;
            for (int i = 0; i < configStruct.marker_num; i++)
            {
                ArManWrap.ARMultiEachMarkerInfoT markerInfo = (ArManWrap.ARMultiEachMarkerInfoT)Marshal.PtrToStructure(currentMarker, typeof(ArManWrap.ARMultiEachMarkerInfoT));
                this.actionMarkerIDs.Add(markerInfo.patt_id);
                currentMarker = (IntPtr)((int)currentMarker + Marshal.SizeOf(markerInfo));
            }


            // Align camera to root tracker
            float[] projMatrix = new float[16];
            ArManWrap.ARTKPGetProjectionMatrix(this.boardTracker, projMatrix);

            double[] matrix = new double[16];
            for (int i = 0; i < 16; i++)
                matrix[i] = projMatrix[i];
            co.AlignCamera(matrix);

            // Subscribe to camera update
            co.SubscribeCameraUpdateHandler(new Coordinator.CameraUpdateHandler(OnCameraUpdate));
        }

        public override void OnCameraUpdate(byte[] buffer)
        {
            CalcResults.Clear();

            int i;

            // Matrix in double format
            double[] dMatrix = new double[16];

            float[] modelViewMatrix = new float[16];

            // Get board matrix

            // if no board detected, do not proceed
            if (ArManWrap.ARTKPCalcMulti(this.boardTracker, buffer) <= 0)
                return;

            ArManWrap.ARTKPGetModelViewMatrix(this.boardTracker, modelViewMatrix);


            // Convert matrix from float to double
            for (i = 0; i < 16; i++)
                dMatrix[i] = modelViewMatrix[i];

            // Add to result array
            ARMarker markerData = new ARMarker();
            markerData.ID = -1;
            markerData.TransMatrix = dMatrix;
            CalcResults.Add(markerData);

            //////////////////////////////////////////////////////////////

            // Detect action markers
            int numMarkers = ArManWrap.ARTKPCalcMulti(this.actionTracker, buffer);
            for (i = 0; i < numMarkers; i++)
            {
                ArManWrap.ARMarkerInfo armi = ArManWrap.ARTKPGetDetectedMarkerStruct(this.actionTracker, i);

                // Skip if this is not action marker
                if (!this.actionMarkerIDs.Contains(armi.id)) continue;

                IntPtr markerInfos = ArManWrap.ARTKPGetDetectedMarker(this.actionTracker, i);
                float[] center = new float[2];
                float width = 40;
                float[] matrix = new float[12];

                ArManWrap.ARTKPGetTransMat(this.actionTracker, markerInfos, center, width, matrix);

                markerData = new ARMarker();
                markerData.ID = armi.id;
                markerData.TransMatrix = MatrixUtil.GetDMatrixFromOGLM(matrix);

                // Add to result array
                CalcResults.Add(markerData);
            }

            // Done calculations, fire update event
            ARUpdateEvent(CalcResults);
        }

        public override void Destroy(Coordinator co)
        {
            CalcResults.Clear();
            CalcResults = null;
            this.actionMarkerIDs.Clear();
            this.actionMarkerIDs = null;

            co.UnSubscribeCameraUpdateHandler(new Coordinator.CameraUpdateHandler(OnCameraUpdate));
        }

        #endregion

        #endregion
    }
}
